/*
 * main.c
 *
 *  Created on: 8 sept. 2014
 *      Author: MarioC
 */

#include <inttypes.h>
#include <stdlib.h>
#include <string.h>

#include "hcpp/ptp2_log.h"
#include "hcpp/pps_pll.h"
#include "hcpp/ptp2_master.h"

#include "hal/register.h"
#include "hal/pps_pll_hal.h"
#include "pll/pid_controller.h"
#include "hcpp/ptp2_log.h"


#define MAX_ERROR_SIZE			1000			// number of input samples

typedef struct {
	uint32_t		sample_num;					// number of samples available
	uint32_t		current_idx;				// index pointing to the current sample
	int32_t			error[MAX_ERROR_SIZE];		// error value storage
	uint32_t		control[MAX_ERROR_SIZE];	// storage of PID control output
} error_data_t;

static uint32_t		sample_num = 0;
static int32_t  	phase_error[MAX_ERROR_SIZE];
static error_data_t simulation_data;

uint32_t 			mock_registers[HOST_PLL_PPS_Status];




void
my_array_init(int32_t array[], int32_t value, uint32_t size)
{
	uint16_t i;
	for ( i = 0; i < size; i++ ){
		array[i] = value;
	}
}

void
mock_registers_init(uint32_t registers[])
{
	av_bus_write_32(HOST_PHASE_MEAS, 0x0);
	av_bus_write_32(HOST_KxA, 0x00040080);
	av_bus_write_32(HOST_KxB, 0x000D0100);
	av_bus_write_32(HOST_SetPoint, 0x0);
	av_bus_write_32(HOST_LimTmax, 0x077359BD);
	av_bus_write_32(HOST_LimTmin, 0x077358C3);
	av_bus_write_32(HOST_Accu, 0x07735940);
	av_bus_write_32(HOST_PULSE_WIDTH, 0x0BEBC200);
	av_bus_write_32(HOST_PLL_CTRL, 0x0000006C);
	av_bus_write_32(HOST_PLL_PPS_Status, (0x07735940 << 4));

}

void
error_generator(error_data_t *simulation_data, uint8_t	test_case)
{
	switch(test_case){
	case 0:
		PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("Test case 0: no error"));
		// simplest case: error always 0
		simulation_data->current_idx = 0;
		simulation_data->sample_num = 10;
		memset(&simulation_data->error, 0x00000000, MAX_ERROR_SIZE);
		break;

	case 1:
		PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("Test case 1: single pulse"));
		// single pulse
		simulation_data->current_idx = 0;
		simulation_data->sample_num = 10;
		my_array_init(&simulation_data->error[0], 0, MAX_ERROR_SIZE);
		simulation_data->error[0] = 10;
		break;

	case 2:
		PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("Test case 2: step = 10"));
		// step of 10
		simulation_data->current_idx = 0;
		simulation_data->sample_num = 20;
		my_array_init(&simulation_data->error[0], 10, MAX_ERROR_SIZE);
		break;

    case 3:
        PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("Test case 3: pulse = 200"));
        // pulse of 200
        simulation_data->current_idx = 0;
        simulation_data->sample_num = 100;
        my_array_init(&simulation_data->error[0], 0, MAX_ERROR_SIZE);
        simulation_data->error[0] = 200;
        break;

    case 4:
        PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("Test case 4: step = 150"));
        // step of 10
        simulation_data->current_idx = 0;
        simulation_data->sample_num = 100;
        my_array_init(&simulation_data->error[0], 150, MAX_ERROR_SIZE);
        break;
	}
}


bool
simulate_read(pps_pll_t *pps_pll, error_data_t *error_data)
{
	bool					result = true;
    host_pll_pps_status_t 	status_reg;
    host_pll_pps_ctrl_t		ctrl_reg;

    if (error_data->current_idx < error_data->sample_num) {

		pps_pll_status_reg_get  (&status_reg);
		pps_pll_ctrl_reg_get	(&ctrl_reg);

		// print out initial register content

		PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_VERBOSE, ("status_reg.\n\tPeriods: %"PRIu32" \n\tLimits reached: %"PRIu32" \n\tOverflow: %"PRIu32" \n\tUpdate: %"PRIu32"",
				status_reg.reg_t.periods,
				status_reg.reg_t.limit_reached,
				status_reg.reg_t.overflow,
				status_reg.reg_t.updated));


		PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_VERBOSE, ("ctrl_reg.\n\tPLL reset: %"PRIu32" \n\tresync: %"PRIu32" \n\tload_acc: %"PRIu32" \n\tAnti wind up: %"PRIu32" \n\tEnabled: %"PRIu32" \n\tUse default: %"PRIu32" \n\tPID set: %"PRIu32"",
				ctrl_reg.reg_t.rstpp,
				ctrl_reg.reg_t.rsync,
				ctrl_reg.reg_t.ldacc,
				ctrl_reg.reg_t.antwu,
				ctrl_reg.reg_t.enreg,
				ctrl_reg.reg_t.usdef,
				ctrl_reg.reg_t.chkx));


		// write simulated data in phase_register
		pps_pll_phase_set(error_data->error[error_data->current_idx]);

		// set flags according to delta_phase
		// 125 corresponds limits
		status_reg.reg_t.limit_reached = (abs(phase_error[sample_num]) > 125) ? 1 : 0;

		PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("status_reg.raw: %"PRIu32"", status_reg.raw));
		pps_pll_status_reg_set(status_reg.raw);
		error_data->current_idx++;
    } else {
    	result = false;
    }

    return result;

}

void
store_control_value(error_data_t *simulation_data, uint32_t control_value)
{
	simulation_data->control[simulation_data->current_idx - 1] = control_value;
}







int
main()
{
	bool					data_available;
    float                   ki;
    float					kp;
    pid_ctrl_t              pps_pll_pid;
    pid_params_t            pid_params;
     pid_limiter_t		    pid_limiter;

    host_pll_pps_status_t 	status_reg;
    host_pll_pps_ctrl_t		ctrl_reg;
	ptp2_master_t			master;

	mock_registers_init(mock_registers);
	ptp2_log_enable();
	ptp2_log_init();

	error_generator(&simulation_data, 4);

    pps_pll_filter_get(&ki, &kp, 0);

    float_to_s3_8_t	(&pid_params.kd, 0.0);
    float_to_s3_8_t	(&pid_params.ki, ki);
    float_to_s3_8_t	(&pid_params.kp, kp);
    pid_params.set_point = 125000000;
    pid_params.anti_windup = true;
    pid_limiter.min = 125000000 - 125;
    pid_limiter.max = 125000000 + 125;
    pid_init(&pps_pll_pid, &pid_params, &pid_limiter);


    pps_pll_status_reg_get  (&status_reg);
    pps_pll_ctrl_reg_get	(&ctrl_reg);

    // print out initial register content
    PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_VERBOSE, ("status_reg.\n\tPeriods: %"PRIu32" \n\tLimits reached: %"PRIu32" \n\tOverflow: %"PRIu32" \n\tUpdate: %"PRIu32"",
    		status_reg.reg_t.periods,
    		status_reg.reg_t.limit_reached,
    		status_reg.reg_t.overflow,
    		status_reg.reg_t.updated));


    PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_VERBOSE, ("ctrl_reg.\n\tPLL reset: %"PRIu32" \n\tresync: %"PRIu32" \n\tload_acc: %"PRIu32" \n\tAnti wind up: %"PRIu32" \n\tEnabled: %"PRIu32" \n\tUse default: %"PRIu32" \n\tPID set: %"PRIu32"",
    		ctrl_reg.reg_t.rstpp,
    		ctrl_reg.reg_t.rsync,
    		ctrl_reg.reg_t.ldacc,
    		ctrl_reg.reg_t.antwu,
    		ctrl_reg.reg_t.enreg,
    		ctrl_reg.reg_t.usdef,
    		ctrl_reg.reg_t.chkx));

    if (!pps_pll_init(&(master.pps_pll_monitor), &master)) {
       return -1;
    }

    do {
    	int32_t					delta_phase;
    	host_pll_pps_status_t	tmp;
    	uint32_t				control_value = 0;

    	// retrieve next sample from simulation data
    	data_available = simulate_read(&master.pps_pll_monitor, &simulation_data);
    	// run pid simulation passing the new value
    	pps_pll_phase_get(&delta_phase);
        control_value = pid_update(&pps_pll_pid, delta_phase, &pid_limiter);

        store_control_value(&simulation_data, control_value);

        // set the pid returned value in the PLL PPS status register and notify update to upper layer
        pps_pll_status_reg_get(&tmp);
        tmp.reg_t.updated = 1;
        tmp.reg_t.periods = control_value;
        pps_pll_status_reg_set  (tmp.raw);

        pps_pll_monitor(&(master.pps_pll_monitor));
        PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("PID error : %"PRIi32", PID Control : %"PRIu32", Integral buffer : %f", master.pps_pll_monitor.delta_1pps, control_value, pps_pll_pid.integral_buffer));


    } while(data_available);

    return 0;
}

